/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "cartographer/mapping/imu_tracker.h"
#include <cmath>
#include <limits>
#include "cartographer/common/math.h"
#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"

namespace cartographer
{
    namespace mapping
    {
        /*
         * 该类的作用是利用重力向量的偏差来纠正四元数。
         * 理解：
         * 1) 先假设初始时，车载坐标系与IMU坐标系处于同一水平面，即只有偏航角。如两个坐标系朝上的是都是Z轴，可以理解两个坐标系Z轴平行。
         * 2) 基于上面假设，不管机器人后面如何旋转，重力方向是不变的，因为重力始终垂直与水平面，但是车载坐标系一直在旋转，其Z轴可能已
         *	 经不垂直这个水平面了，即车载系的Z轴已经不跟重力方向水平了。
         * 3) 机器人的当前姿态(四元数)表示机器人相对于初始状态旋转了多少，那么这个四元数的逆就是反向旋转，这个反向旋转可以把机器人姿态
         *	 恢复到初始状态，那么反向旋转乘以一个Z轴的单位向量，就是把当前车载系的Z轴反向转到初始状态，这个相乘后的结果就是函数
         *	 AddImuLinearAccelerationObservation中的orientation_.conjugate() * Eigen::Vector3d::UnitZ()的结果，那么这个结果在理想情况下
         *    应该是和重力向量平行的，这个理想情况指的是我们根据角速度积分得到orientation_没有误差，即机器人姿态没有误差。但是实际不可能
         *	 没有误差的。求这个误差就是把重力向量和反转后的那个向量进行比较，看差了多少，对应的代码是
         *	 FromTwoVectors(gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ())
         * 4) 根据上面求得的误差来修正当前姿态，对应代码orientation_ = (orientation_ * rotation).normalized();
         * 5) 这个重力向量gravity_vector_也要不断更新的，因为我们上面求的误差是相对误差，即上时刻与当前时刻的旋转误差。如果不更新，就把累
         *    计误差都包含进来了。
         * 6) gravity_vector_的更新代码是gravity_vector_ = rotation.conjugate() * gravity_vector_;即用两时刻的旋转变化量来更新重力向量。
         * 7) 注意上面对gravity_vector_的旋转更新量是利用上时刻的角速度估计得来的，在纠正时还要先跟IMU测量的加速度进行融合。
         * 8) 注意在1)中假设在初始时，车载坐标系的Z轴与IMU系的重力向量方向水平，但实际应用中，机器人运动的平面不是IMU系所在平面，即机器人
         *    运动的平面可能是倾斜的，所以实际应用中在初始化时要计算这个倾斜的角度。
         *
         * 总结：用机器人当前姿态四元数的反向旋转的Z轴分量与重力向量对比得到误差，用误差修正当前姿态四元数。重力向量不是一成不变的，用中
         *		指数平均法来求当前的重力向量。
         *
         */
        ImuTracker::ImuTracker(const double imu_gravity_time_constant, const common::Time time)
            : imu_gravity_time_constant_(imu_gravity_time_constant),
              time_(time),
              last_linear_acceleration_time_(common::Time::min()),
              orientation_(Eigen::Quaterniond::Identity()),
              gravity_vector_(Eigen::Vector3d::UnitZ()),
              imu_angular_velocity_(Eigen::Vector3d::Zero())
        {
        }

        void ImuTracker::Advance(const common::Time time)
        {
            CHECK_LE(time_, time);
            const double delta_t = common::ToSeconds(time - time_);
            // 根据上时刻角速度积分估计到当前时刻(time)的旋转变化量
            const auto rotation = transform::AngleAxisVectorToRotationQuaternion(Eigen::Vector3d(imu_angular_velocity_ * delta_t));
            // 根据上时刻姿态乘以旋转变化量得到当前时刻的姿态
            orientation_ = (orientation_ * rotation).normalized();
            // conjugate就是逆向旋转，用两时刻间的旋转变化量不断更新重力向量，在没有误差的情况下，这个重力向量应该始终垂直与IMU系
            // 所在的水平面
            gravity_vector_ = rotation.conjugate() * gravity_vector_;
            time_ = time;
        }

        /* 2023/06/07 accelerater output is gravity vector in stationery or average speed status.
         * the result of orientation conjugate represents inverse rotation. the vector of inverse rotation in z axis coincides with
         * gravity vector.
         */

        /*https://www.cnblogs.com/gary-guo/p/16608604.html
        imu器件测量的原理：重力和IMU器件之间作用力相反，重力向下，IMU测量值朝上.
        */
        void ImuTracker::AddImuLinearAccelerationObservation(const Eigen::Vector3d &imu_linear_acceleration)
        {
            // Update the 'gravity_vector_' with an exponential moving average using the 'imu_gravity_time_constant'.
            const double delta_t = last_linear_acceleration_time_ > common::Time::min()
                                       ? common::ToSeconds(time_ - last_linear_acceleration_time_)
                                       : std::numeric_limits<double>::infinity();
            last_linear_acceleration_time_ = time_;
            // delta_t越大, alpha越大
            const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
            // 将积分估计重力向量与当前传入的线加速度进行融合(采用指数滑动平均法)得到新的重力向量
            gravity_vector_ = (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
            // 求重力向量与当前姿态在Z轴的反向旋转量之间的误差
            const Eigen::Quaterniond rotation = FromTwoVectors(gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());
            // 利用旋转误差纠正当前姿态
            orientation_ = (orientation_ * rotation).normalized();
            CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
            CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
        }

        void ImuTracker::AddImuAngularVelocityObservation(const Eigen::Vector3d &imu_angular_velocity)
        {
            imu_angular_velocity_ = imu_angular_velocity;
        }
    } // namespace mapping
} // namespace cartographer
